/**
 * M5麦克纳姆轮遥控小车M5麦克纳姆轮遥控小车
 * 
 * https://www.xpstem.com/maker/m5-mecanumcar
 * author: billy zhang
 */

#include "M5Stack.h"
#include "LegoMotor.h"

#define MAX(in, max) (((in) > (max)) ? (in) : (max))
#define MIN(in, min) (((in) < (min)) ? (in) : (min))
#define CONSTRAIN(in, min, max) MAX(min, MIN(in, max))

#define BASE_X_PWM_DUTY_ADDR    (0x20)
#define BASE_X_ENCODER_ADDR      (0x30)
#define BASE_X_SPEED_ADDR       (0x40)
/*
   |  0  |       1     |      2     |     3      |    4, 5, 6, 7  |          8         |     9    |    10   |    11   |     12      |          
   | mod |  position-p | position-i | position-d | position-point | position-max-speed |  speed-p | speed-i | speed-d | speed-point | 
*/
#define BASE_X_CONFIG_ADDR      (0x50)

/**
 * @description: 
 * @param mode: NORMAL, POSITION LOCK, SPEED LOCK
 * @return: None
 */
void LegoMotor::SetMode(uint8_t mode)
{    
    M5.I2C.writeByte(baseAddr_, BASE_X_CONFIG_ADDR + (0x10 * pos_), mode);
}   

/**
 * @description: 
 * @return: encoder value
 */
int32_t LegoMotor::GetEncoderValue()
{
    uint8_t read_buf[4] = {0, 0, 0, 0};
    uint8_t addr = BASE_X_ENCODER_ADDR + 4 * pos_;
    M5.I2C.readBytes(baseAddr_, addr, 4, read_buf);
    return (read_buf[0] << 24) | (read_buf[1] << 16) | (read_buf[2] << 8) | read_buf[3];
}

/**
 * @description: 
 * @param encoder: INT32_MIN ~ INT32_MAX 
 * @return: None 
 */
void LegoMotor::SetEncoderValue(int32_t encoder)
{
    uint8_t write_buf[4] = {0, 0, 0, 0};
    uint8_t addr = BASE_X_ENCODER_ADDR + (4 * pos_);
    
    write_buf[0] = encoder >> 24;
    write_buf[1] = encoder >> 16;
    write_buf[2] = encoder >> 8;
    write_buf[3] = encoder & 0xff;

    M5.I2C.writeBytes(baseAddr_, addr, write_buf, 4);
}

/**
 * @description: 
 * @param duty: Set motor speed, -127 ~ 127
 * @return: 
 */
void LegoMotor::SetMotorSpeed(int8_t duty)
{
    uint8_t addr = BASE_X_PWM_DUTY_ADDR + pos_;

    M5.I2C.writeByte(baseAddr_, addr, duty);
}

/**
 * @description: 
 * @return Motor run speed, -127 ~ 127: 
 */
int8_t LegoMotor::GetMotorSpeed()
{
    uint8_t read_data;
    uint8_t addr = BASE_X_PWM_DUTY_ADDR + pos_;

    M5.I2C.readByte(baseAddr_, addr, &read_data);
    return read_data;
}


/**
 * @description: 
 * @return: Motor encoder increments every 20 ms
 */
int8_t LegoMotor::GetMotorSpeed20MS()
{
    uint8_t read_data;
    uint8_t addr = BASE_X_SPEED_ADDR + pos_;

    M5.I2C.readByte(baseAddr_, addr, &read_data);
    return read_data;
}

void LegoMotor::SetPositionPID(uint8_t kp, uint8_t ki, uint8_t kd)
{
    uint8_t write_buf[3] = {0, 0, 0};
    uint8_t addr = BASE_X_CONFIG_ADDR + (pos_ * 0x10) + 0x01;
    write_buf[0] = kp;
    write_buf[1] = ki;
    write_buf[2] = kd;

    M5.I2C.writeBytes(baseAddr_, addr, write_buf, 3);
}   

/**
 * @description: 
 * @param position_point: in POSITION mode, motor will lock in this value, INT32_MIN ~ INT32_MAX
 * @return: None
 */
void LegoMotor::SetPositionPoint(int32_t position_point)
{
    uint8_t write_buf[4] = {0, 0, 0, 0};
    uint8_t addr = BASE_X_CONFIG_ADDR + (pos_ * 0x10) + 0x04;
    
    write_buf[0] = position_point & 0xff;
    write_buf[1] = position_point >> 8;
    write_buf[2] = position_point >> 16;
    write_buf[3] = position_point >> 24;

    // Serial.printf(" %x %x %x %x \r\n", write_buf[0], write_buf[1], write_buf[2], write_buf[3]);
    M5.I2C.writeBytes(baseAddr_, addr, write_buf, 4);
}

/**
 * @description: 
 * @param  max_pwm: 0 ~ 127, POSITION mode, max speed  
 * @return: 
 */
void LegoMotor::SetPostionPIDMaxSpeed(uint8_t max_pwm)
{
    uint8_t addr = BASE_X_CONFIG_ADDR + (pos_ * 0x10) + 0x08;

    M5.I2C.writeByte(baseAddr_, addr, max_pwm);
}

void LegoMotor::SetSpeedPID(uint8_t kp, uint8_t ki, uint8_t kd)
{
    uint8_t write_buf[3] = {0, 0, 0};
    uint8_t addr = BASE_X_CONFIG_ADDR + (pos_ * 0x10) + 0x09;
    write_buf[0] = kp;
    write_buf[1] = ki;
    write_buf[2] = kd;

    M5.I2C.writeBytes(baseAddr_, addr, write_buf, 3);
}

/**
 * @description: 
 * @param speed_point: speed_point is lock GetMotorSpeed20MS(), not GetMotorSpeed(), maybe -20 ~ 20
 * @return: None
 */
void LegoMotor::SetSpeedPoint(int8_t speed_point)
{
    uint8_t addr = BASE_X_CONFIG_ADDR + (pos_ * 0x10) + 0x0c;

    M5.I2C.writeByte(baseAddr_, addr, (uint8_t)speed_point);
}
